/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *  V1.2.0     11-25- 2021     yanfubing       1.add engineer can 1
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */
#include "CAN_receive.h"
#include "cmsis_os.h"
#include "main.h"
#include "bsp_rng.h"
#include "detect_task.h"
#include "remote_control.h"

extern CAN_HandleTypeDef hcan1;      
extern CAN_HandleTypeDef hcan2;
//motor data read
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }

/*
电机数据:   
		   CAN1		                                   by  江宽
	0：pitch电机    2006
	1: 夹具旋转电机左3508
	2：夹具        右3508
	3：横向臂电机   3508
	       CAN2
	0：摩擦轮电机前 2006电机
	1：摩擦轮    后 2006
	2：摩擦轮    左 2006

*/

static motor_measure_t motor_clamp[3];
static motor_measure_t motor_friction[4];

static CAN_TxHeaderTypeDef  engineering_tx_message;
static uint8_t              engineering_can_send_data[8];          //夹取  云台
static CAN_TxHeaderTypeDef  friction_tx_message;
static uint8_t              friction_can_send_data[8];             //摩擦轮 

static CAN_TxHeaderTypeDef  chassis_A_tx_message;
static uint8_t              chassis_A_can_send_data[8];

 can_receive_t can_chassis_data;

/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    if (hcan == &hcan2)
    {
        switch (rx_header.StdId)
        {		
			case CAN_CHASSIS_A_ID:								 //具体数据后续待定
			{
                can_chassis_data.button_limit_flg = rx_data[0];
				can_chassis_data.top_limit_flg = rx_data[1];
				detect_hook(chassis_A_TOE);
				break;
			}
			default:
			{
				break;
			}
        }
    }
    else if (hcan == &hcan1)  
    {
        switch (rx_header.StdId)
        {
			case CAN_translation_MOTOR_ID:
			case CAN_Clamp_turn_MOTOR1_ID:
			case CAN_Clamp_turn_MOTOR2_ID:
			{
				static uint8_t i = 0;
				i = rx_header.StdId - CAN_translation_MOTOR_ID;
				get_motor_measure(&motor_clamp[i], rx_data);
				//目前计算一个电机角度
				//get_clamp_ecd_conut_R(&motor_clamp[2]);
				//get_clamp_ecd_conut_L(&motor_clamp[2]);
				// if (HAL_GPIO_ReadPin(M1_GPIO_Port, M1_Pin))
				// {
				// 	motor_clamp[2].ecd_count = 0;
				// }
				if (motor_clamp[i].ecd - motor_clamp[i].last_ecd > HALF_ECD_RANGE)
				{
					motor_clamp[i].ecd_count--;
				}
				else if (motor_clamp[i].ecd - motor_clamp[i].last_ecd < -HALF_ECD_RANGE)
				{
					motor_clamp[i].ecd_count++;
				}
				detect_hook(translation_motor_TOE + i);
				break;
			}
			case CAN_friction_MOTOR1_ID:
			case CAN_friction_MOTOR2_ID:
			case CAN_friction_MOTOR3_ID:
			case CAN_friction_MOTOR4_ID:
			{
				static uint8_t i = 0;
				i = rx_header.StdId - CAN_friction_MOTOR1_ID;
				get_motor_measure(&motor_friction[i], rx_data);
				detect_hook(friction_MOTOR1_TOE + i);
				break;
			}

			default:
			{
				break;
			}
        }
    }
}


/**
  * @brief          向底盘A板发送控制数据     by 江宽		发送数据待定
  * @param[in]      待发送的数据					
  * @retval         none
  */
void CAN_cmd_chassis_C(can_transmit_t* can_transmit_data)
{
	uint32_t send_mail_box;
	chassis_A_tx_message.StdId = CAN_gimbal_A_ID;
	chassis_A_tx_message.IDE = CAN_ID_STD;
	chassis_A_tx_message.RTR = CAN_RTR_DATA;
	chassis_A_tx_message.DLC = 0x08;
	chassis_A_can_send_data[0] = can_transmit_data->x_speed;
	chassis_A_can_send_data[1] = can_transmit_data->y_speed;
	chassis_A_can_send_data[2] = can_transmit_data->z_speed;
	chassis_A_can_send_data[3] = can_transmit_data->go_down_speed;        
	chassis_A_can_send_data[4] = can_transmit_data->state_flag;     //高三位对应底盘的6种状态，4、5位对应三种工作模式：救援，取矿，兑换
	chassis_A_can_send_data[5] = can_transmit_data->cail_flg;       //高两位，0x7F对应底盘救援校准，0xBF底盘校准
	chassis_A_can_send_data[6] = can_transmit_data->rc_to_cahssis->key.v >> 8;
	chassis_A_can_send_data[7] = can_transmit_data->rc_to_cahssis->key.v;
	HAL_CAN_AddTxMessage(&chassis_CAN, &chassis_A_tx_message, chassis_A_can_send_data, &send_mail_box);
}


/**
  * @brief          发送 夹具旋转 横向臂 电机控制电流(0x201)
  * @param[in]      motor_2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_engineering(int16_t motor_translation,int16_t motor_Clamp_turn1, int16_t motor_Clamp_turn2)
{
	uint32_t send_mail_box;
	engineering_tx_message.StdId = CAN_ENGINEER_ALL_ID;
	engineering_tx_message.IDE = CAN_ID_STD;
	engineering_tx_message.RTR = CAN_RTR_DATA;
	engineering_tx_message.DLC = 0x06;
	engineering_can_send_data[0] = motor_translation >> 8;
	engineering_can_send_data[1] = motor_translation;
	engineering_can_send_data[2] = motor_Clamp_turn1 >> 8;
	engineering_can_send_data[3] = motor_Clamp_turn1;
	engineering_can_send_data[4] = motor_Clamp_turn2 >> 8;
	engineering_can_send_data[5] = motor_Clamp_turn2;
	engineering_can_send_data[6] = 0;
	engineering_can_send_data[7] = 0;

	HAL_CAN_AddTxMessage(&obtein_CAN, &engineering_tx_message, engineering_can_send_data, &send_mail_box);
}
/**
  * @brief          发送摩擦轮电机控制电流
  * @param[in]      motor_1: (0x205) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_2: (0x206) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_3: (0x207) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_4: (0x208 ) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_friction(int16_t motor_friction_1, int16_t motor_friction_2, int16_t motor_friction_3, int16_t motor_friction_4)
{
	uint32_t send_mail_box;
	friction_tx_message.StdId = CAN_friction_ALL_ID;
	friction_tx_message.IDE = CAN_ID_STD;
	friction_tx_message.RTR = CAN_RTR_DATA;
	friction_tx_message.DLC = 0x08;
	friction_can_send_data[0] = motor_friction_1 >> 8;
	friction_can_send_data[1] = motor_friction_1;
	friction_can_send_data[2] = motor_friction_2 >> 8;
	friction_can_send_data[3] = motor_friction_2;
	friction_can_send_data[4] = motor_friction_3 >> 8;
	friction_can_send_data[5] = motor_friction_3;
	friction_can_send_data[6] = motor_friction_4 >> 8;
	friction_can_send_data[7] = motor_friction_4;

	HAL_CAN_AddTxMessage(&friction_CAN, &friction_tx_message, friction_can_send_data, &send_mail_box);
}

/**
  * @brief          返回夹具电机 3508电机数据指针           
  * @param[in]      i: 电机编号,范围[1，2]
  * @retval         电机数据指针
  */
const motor_measure_t* get_Clamp_turn_motor_measure_point(uint8_t i)
{
	return &motor_clamp[i+1];
}

/**
  * @brief          返回横向臂电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[3]
  * @retval         电机数据指针
  */
const motor_measure_t* get_translation_motor_measure_point()
{
	return &motor_clamp[0];
}
/**
  * @brief          返回摩擦轮电机 2006电机数据指针
  * @param[in]      i: 电机编号,范围[0，2]
  * @retval         电机数据指针
  */
const motor_measure_t* get_friction_motor_measure_point(uint8_t i)
{
	return &motor_friction[i];
}

/**
  * @brief          返回底盘数据
  * @param[in]      i: 
  * @retval         电机数据指针
  */
const can_receive_t* get_chassis_A_point()
{
	return &can_chassis_data;
}


